/*******************************************************************************
* Copyright 2017 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

/* Author: Ryu Woon Jung (Leon) */

#include "port_handler.h"
#include "stm32f4xx.h"

#define DXL_BAUDRATE 2000000

#define DXL_TX_PIN			GPIO_Pin_9
#define DXL_TX_PinSource    GPIO_PinSource9

#define DXL_RX_PIN			GPIO_Pin_10
#define DXL_RX_PinSource    GPIO_PinSource10

#define DXL_CTRL_PIN		GPIO_Pin_11

#define	DXL_PORT			GPIOA
#define	DXL_IO_CLK_EN		RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE)

#define DXL_UART			USART1
#define DXL_UART_CLK_EN		RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE)

#define DXL_GPIO_AF_UART 		GPIO_AF_USART1

uint8_t	dxl_timeout_ms=10;
uint8_t dxl_current_ms=0;

void initPort()
{
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	DXL_UART_CLK_EN;
	DXL_IO_CLK_EN;

	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;

	GPIO_InitStructure.GPIO_Pin = DXL_TX_PIN;
	GPIO_Init(DXL_PORT, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin = DXL_RX_PIN;
	GPIO_Init(DXL_PORT, &GPIO_InitStructure);

	GPIO_PinAFConfig(DXL_PORT,DXL_TX_PinSource,DXL_GPIO_AF_UART);
	GPIO_PinAFConfig(DXL_PORT,DXL_RX_PinSource,DXL_GPIO_AF_UART);

	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_Pin = DXL_CTRL_PIN;
	GPIO_Init(DXL_PORT, &GPIO_InitStructure);

	USART_InitStructure.USART_BaudRate 	 = DXL_BAUDRATE;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits   = USART_StopBits_1;
	USART_InitStructure.USART_Parity     = USART_Parity_No ;
	USART_InitStructure.USART_Mode 	    = USART_Mode_Rx | USART_Mode_Tx;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;

	USART_Init(DXL_UART,&USART_InitStructure);
	USART_Cmd(DXL_UART,ENABLE);
	g_is_using = False;
}

int readPort (uint8_t *packet, int length)
{
	uint32_t i=0, cnt=0;
	GPIO_ResetBits(DXL_PORT, DXL_CTRL_PIN);
	for(i=0; i<length; i++)
	{
		if(USART_GetFlagStatus(DXL_UART, USART_FLAG_RXNE) == SET)
		{
			*(packet+cnt) = USART_ReceiveData(DXL_UART);
			cnt++;
		}
	}
	return cnt;
}

int writePort (uint8_t *packet, int length)
{
	uint32_t i;
	GPIO_SetBits(DXL_PORT, DXL_CTRL_PIN);
	for(i=0; i<length; i++)
	{
		USART_SendData(DXL_UART, *(packet+i));
		while(USART_GetFlagStatus(DXL_UART, USART_FLAG_TC) == RESET){}
	}
	GPIO_ResetBits(DXL_PORT, DXL_CTRL_PIN);
	return length;
}

void setPacketTimeout(uint16_t packet_length)
{
	dxl_current_ms = 0;
	dxl_timeout_ms = packet_length>>4;
}

uint8_t isPacketTimeout()
{
	return (dxl_current_ms>dxl_timeout_ms);
}
